#ifndef BTL_MATHS_GAUSSJORDANELIMINATION
#define BTL_MATHS_GAUSSJORDANELIMINATION

#include <Eigen/Core>
#include <limits>
#include <cassert>

namespace btl
{
namespace maths
{

template <typename MatrixT>
class GaussJordanElimination
{
   public:
      typedef typename MatrixT::Scalar Scalar;
      typedef
         Eigen::Matrix<
            typename MatrixT::Scalar,
            MatrixT::RowsAtCompileTime,
            MatrixT::ColsAtCompileTime,
            0,
            MatrixT::MaxRowsAtCompileTime,
            MatrixT::MaxColsAtCompileTime
         > ResultType;

      GaussJordanElimination();
      GaussJordanElimination(const MatrixT& m);

      void compute(const MatrixT& m);

      const ResultType& matrix() const;

   private:
      void reduce();

      ResultType _rowEchelonForm;
};

} // namespace maths
} // namespace btl

namespace btl
{

using maths::GaussJordanElimination;

} // namespace btl

// ====================================================================
// === Implementation

namespace btl
{
namespace maths
{

template <typename MatrixT>
inline GaussJordanElimination<MatrixT>::GaussJordanElimination()
{
}

template <typename MatrixT>
inline GaussJordanElimination<MatrixT>::GaussJordanElimination(const MatrixT& m):
   _rowEchelonForm(m)
{
   reduce();
}

template <typename MatrixT>
inline void GaussJordanElimination<MatrixT>::compute(const MatrixT& m)
{
   _rowEchelonForm = m;
   reduce();
}


template <typename MatrixT>
inline const typename GaussJordanElimination<MatrixT>::ResultType&
      GaussJordanElimination<MatrixT>::matrix() const
{
   return _rowEchelonForm;
}

template <typename MatrixT>
inline void GaussJordanElimination<MatrixT>::reduce()
{
   ResultType& M = _rowEchelonForm;
   const int rows = M.rows();
   const int cols = M.cols();

   // compute a reasonable zero-tolerance value
   // uses formula from Octave's rref() function
   // FIXME: tolerance should be a parameter of the algorithm

   const Scalar eps = std::numeric_limits<Scalar>::epsilon();
   // eps * max (rows, cols) * norm (A, inf);
   const Scalar tolerance
      = eps * Scalar(std::max(rows, cols))
      * M.array().abs().colwise().maxCoeff().sum();

   //std::cout << "tolerance = " << tolerance << "\n";

   Eigen::Matrix<Scalar, 1, MatrixT::ColsAtCompileTime> pivotTmp;

   int row = 0;
   for (int col = 0; row < rows && col < cols; ++col)
   {
      assert(row < rows);

      // choose pivot (row containing maximum value)
      int pivot = -1;
      Scalar maxVal = -std::numeric_limits<Scalar>::infinity();
      for (int i = row; i < rows; ++i)
      {
         const Scalar v = std::abs(M(i, col));
         assert(v > -std::numeric_limits<Scalar>::infinity());

         if (v > maxVal)
         {
            pivot = i;
            maxVal = v;
         }
      }

      assert(maxVal > -std::numeric_limits<Scalar>::infinity());
      assert(pivot >= 0);

      if (maxVal <= tolerance)
      {
         // max. row in this column is nearly zero
         // just completely zero-out remaining values in this column
         M.block(row, col, rows-row, 1).setZero();

         // FIXME:
         //  the Octave rref() implementation doesn't implement r here...
         //  but is that correct??
      }
      else
      {
         // swap pivot in
         if (pivot != row)
         {
            pivotTmp = M.row(pivot);
            M.row(pivot) = M.row(row);
            M.row(row) = pivotTmp;
         }

         // normalize pivot row
         M.row(row) /= M(row, col);

         // eliminate column
         for (int i = 0; i < rows; ++i)
         {
            if (i == row) continue;
            M.row(i) -= M(i,col) * M.row(row);
         }

         // this row is used
         ++row;
      }
   }
}

} // namespace maths
} // namespace btl

#endif // BTL_MATHS_GAUSSJORDANELIMINATION
